#!/usr/bin/env python
# -*- coding: utf-8 -*-
# @Time    : 2018\9\5 0005 16:29
# @Author  : never mind
# @File    : move_out_policy.py
import copy
import utils_list
import constants as cons
import utils
import select_path
import numpy as np
'''
移车策略
1, 对于被挡车辆，先将其周围的车移除，然后再把target移除
2、按照指定的轨迹将target移除，首先将指定轨迹上的车移开，然后再把target移除

问题：将指定轨迹上的车如何移开？将周围的车如何移开？
'''
def init_trace():
    '''
    初始化所有点的路径，
    # trace 的形式应该是 all_trace: {'point': [[pos], [pos], [pos], [pos]]}
    all_trace = {'1_2': [[[5, 0], [5, 1], [4, 1], [3, 1], [2, 1], [1, 1], [1, 2]]]}
    all_trace = {'2_7': [[[5, 0], [4, 0], [3, 0], [2, 0], [2, 1], [2, 2], [2, 3], [2, 4], [2, 5], [2, 6], [2, 7]]]}

    :return:
    '''
    for i in range(cons.axis_width):
        pass
        for i in range(cons.axis_height):
            pass

    pass
def policy_one(init_position_copied, out_copied,occ):
    stage = []
    # 寻找新的路径
    free_pos, free_trace, except_pos, except_trace = get_new_trace(init_position_copied, out_copied, occ)

    # 移动到 要移开的 target 的临近小车
    car_to_car = select_path.select_path(init_position_copied, except_pos, occ, state="remove")
    stage.append(car_to_car)

    # 将临近小车移到 free_pos
    stage.append(free_trace)

    # 从 free_pos 移动到 target 路径
    free_trace_reverse = utils_list.get_reverse_list(free_trace)
    free_trace_reverse.append(out_copied)
    stage.append(free_trace_reverse)

    # 将 target 移到出口
    stage.append(except_trace)
    # occupied 变化
    # occupied.remove(except_pos)     # except_pos 相当于 remove_car_pair 将目标移出
    # occupied.append(free_pos)

    return stage

def get_new_trace(init_position, out, occ_):
    '''

    :param init_position:
    :param out:
    :param occ:
    :return:
        free_pos            # 临近节点移动到最小开销对应的空闲位置
        free_trace          # 临近节点移动到其他空闲位置的轨迹
        except_pos          # 被移动的临近节点
        except_trace        # 移除临近节点之后，将目标移除的路径
    '''
    up, down, left, right = utils.get_neighbors(out[0], out[1])
    min_score =  10000.0
    free_pos_, free_trace_, except_pos_, except_trace_ = '', '', '', ''
    occ = copy.copy(occ_)

    for item in [up, down, left, right]:
        if not item:
            continue
        else:
            occ_temp = utils_list.get_copied_list(occ, item, 'remove')   # 假设已经移除该临近节点
            # 移除该临近节点之后，该路是通了，得到通了之后的路径和开销
            # 距离出口的路径和开销
            except_score, except_pos, except_trace = utils.get_min_cost_position(out, cons.pos_out, occ_temp)

            # 移除这个点之后，会有一个路径，在free队列上，不能包含这个路径上的点，所以occ需要加上free队列
            extend_temp = utils_list.extend_occupid(occ, except_trace)
            free_queue = utils.construct_free_queue(extend_temp)

            # 将该点移除到其他位置
            free_score, free_pos, free_trace = utils.get_min_cost_position(item, free_queue, occ)     # 假设能够移出到路径之外的位置

            # 总开销
            score = except_score + free_score

            if score < min_score:
                min_score = score
                free_pos_ = free_pos     # 该临近节点应该移到的位置
                free_trace_ = free_trace
                except_pos_ = item
                except_trace_ = except_trace        # 移出的轨迹

    return free_pos_, free_trace_, except_pos_, except_trace_

def policy_two(out_copied, occ, all_trace):
    # 根据原始停车路径获取移出轨迹
    trace = utils.get_trace(out_copied, all_trace=all_trace)

    if not trace:           # 没有该路径
        print("There is no trace for point %s" % out_copied)
        return False

    # 检查路径上是否存在障碍
    all_trace_points, obstacle = utils.check_obstacle(trace, occ)

    if not obstacle:        # 没有障碍点
        print('get the trace!')
        return trace        # [[[], [], []], [[], [], []]]  len(trace) == 2
    else:                   # 存在障碍点
        # 构建循环移出 target 直到路径上没有障碍点
        trace = utils.circle_move()
        return trace

def get_queue_position():
    '''
    选取 要移出小车的位置。维护一个出车队列，按照先入先出的策略。（被否定）
    :return:
    '''
    print("denied!! no such policy.")
    pass

def get_random_position(occupied):
    '''
    从被占有的位置中选择一个作为移出位置
    :param occupied:
    :return:
    '''
    flag = cons.OUT
    length = len(occupied)
    index = np.random.randint(0, length)
    out_pos = occupied[index]
    return flag, out_pos
    pass

def get_cost(path, use_important_level=False):
    '''
    出车的消耗：
    - 只移动平板车的开销，和距离有关
    - 小车在平板车上时，移动平板车的开销，和距离有关
    - 将小车抬起，放下的开销
    - 时间开销：只移动平板车的时间开销，小车在平板车上时，移动小车的时间开销

    :param path:
    :param use_important_level:
    :return:
    '''
    energy_cost = 0
    distance_cost = 0
    time_cost = 0

    move_flat_car_with_car, move_flat_car_only = path[0], path[1]

    # flat
    move_flat_car_with_car_flat = utils_list.flat_list(move_flat_car_with_car)  # 平板车单独移动时候的
    move_flat_car_only_flat = utils_list.flat_list(move_flat_car_only)
    # 只移动平板车
    distance, time_ = utils.compute_distance_score(move_flat_car_only_flat,
                                                  use_important_level=cons.use_important_level)
    distance_cost += distance
    time_cost += time_
    # 取车次数
    energy_cost += cons.cost_energy_up * len(move_flat_car_only)
    # 放车次数
    energy_cost += cons.cost_energy_down * len(move_flat_car_only)

    # 只移动平板车
    for item in move_flat_car_only:
        # item: [[], [], []]
        energy_cost += cons.cost_energy_move_without_car * len(item)
        pass
    # 移动 平板车 和 车
    for item in move_flat_car_with_car:
        energy_cost += cons.cost_energy_move_with_car * len(item)
    # 移动 平板车 和 车
    distance, time_ = utils.compute_distance_score(move_flat_car_with_car_flat,
                                                   use_important_level=cons.use_important_level)
    distance_cost += distance
    time_cost += time_

    total_cost = distance_cost + time_cost + energy_cost

    print('distance_cost: %s, time_cost: %s, energy_cost: %s, total cost: %s'
          % (distance_cost, time_cost, energy_cost, total_cost))

    return total_cost
    pass

def get_cost_no_obstrcal(path, use_important_level=False):
    # 入车的开销：
    # 1、	将小车送到目标位置的距离、时间上的开销
    # 2、	将小车放下的开销	，也就是能量开销
    # 3、   reset 平板车的开销
    distance_cost, time_cost = utils.compute_distance_score(path, use_important_level=use_important_level)
    # energy_cost = utils.computer_energy_score(path)
    energy_cost = 0
    energy_cost += cons.cost_energy_up  # 取车
    energy_cost += cons.cost_energy_down # 放车
    energy_cost += cons.cost_energy_move_with_car * len(path)
    energy_cost += cons.cost_energy_move_without_car * len(path)
    total_cost = distance_cost + time_cost + energy_cost
    print('distance_cost: %s, time_cost: %s, energy_cost: %s, total cost: %s'
          % (distance_cost, time_cost, energy_cost, total_cost))
    return total_cost